import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    创建一个带有两个使用外部控制逻辑的机器人仿真环境
    为每个机器人配置一个外部控制器和轨迹跟踪器
    创建局部路径规划器，生成局部轨迹
    """
    self_package_path = get_package_share_directory("synthesize")
    rviz__file = os.path.join(self_package_path, "config", "dock_view.rviz")

    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim_multi.launch.py"]
            )
        )
    )

    controller1 = GroupAction(
        actions=[
            PushROSNamespace("robot1"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("synthesize"),
                            "launch",
                            "controller_pid_mpc.launch.py",
                        ]
                    )
                ),
                launch_arguments={"vec": "1.0"}.items(),
            ),
        ]
    )

    controller2 = GroupAction(
        actions=[
            PushROSNamespace("robot2"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("synthesize"),
                            "launch",
                            "controller_pid_mpc.launch.py",
                        ]
                    )
                ),
                launch_arguments={"vec": "-1.0"}.items(),
            ),
        ]
    )

    local_path_planer1 = Node(
        package="path_planer",
        executable="local_path_planer",
        name="local_path_planer_node1",
        output="screen",
        parameters=[
            {"radius": 0.1},
            {"invert": False},
            {"start_vec": 0.5},
            {"dock_vec": 0.1},
        ],
        remappings=[
            ("odom_start", "robot1/sp_odom"),
            ("odom_end", "robot2/sp_odom"),
            ("local_path", "robot1/reference_path"),
            ("local_vec", "robot1/reference_velocity"),
        ],
    )

    local_path_planer2 = Node(
        package="path_planer",
        executable="local_path_planer",
        name="local_path_planer_node2",
        output="screen",
        parameters=[
            {"radius": 0.1},
            {"invert": True},
            {"start_vec": 0.5},
            {"dock_vec": 0.1},
        ],
        remappings=[
            ("odom_start", "robot2/sp_odom"),
            ("odom_end", "robot1/sp_odom"),
            ("local_path", "robot2/reference_path"),
            ("local_vec", "robot2/reference_velocity"),
        ],
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz__file],
    )

    return LaunchDescription(
        [
            webotsSim,
            controller1,
            controller2,
            local_path_planer1,
            local_path_planer2,
            rviz2_node,
        ]
    )
